#ifndef CTCPSENDTHREAD_HPP
#define CTCPSENDTHREAD_HPP

#include "SimpleThread.h"
#include <arpa/inet.h>
#include <fcntl.h>
#include <iostream>
#include <netinet/in.h>
#include <poll.h>
#include <pthread.h>
#include <queue>
#include <sched.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>

#include <ifaddrs.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <net/if.h>

#include "common.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <asm/byteorder.h>
#include <linux/ip.h>
#include <netinet/tcp.h>

using namespace cv;
using namespace std;

#define MESSAGE_SIZE 19
#define PORT 8899

class CTcpSendThread : public SimpleThread 
{
public:	
  CTcpSendThread();
  ~CTcpSendThread();
  void DivideGroup(const vector<Point2f> &pts);
  void TcpMsgSender(const vector<Point2f> &pts, int idx);
  void SendPoints(const vector<Point2f> &pts, int idx);
  void Stop();
  int Run(); 
  int CheckTcpAlive(int accept_fd);
  char ** addr;

private:
	int mDstNum = 5;  
  char mSendBuf[MESSAGE_SIZE];
  int socket_fd = -1;
  int flag = 1;
  int backlog = 10;
  // vector<int> maccept_fdVec(mDstNum);
	int maccept_fdVec[5] = {-1, -1, -1, -1, -1};
  int ret = 0;
  struct sockaddr_in local_addr, remote_addr;
};

CTcpSendThread::CTcpSendThread() 
{
  // create a tcp socket
  socket_fd = socket(AF_INET,SOCK_STREAM,0);
  if (socket_fd==-1)
  {
    perror("create tcp-socket error");
    exit(1);
  }
  //set option of socket
  ret = setsockopt(socket_fd,SOL_SOCKET,SO_REUSEADDR,
                     &flag,sizeof(flag));
  if ( ret == -1 )
  {
    perror("set tcp sockopt error");
  }
  //set socket address
  local_addr.sin_family = AF_INET;
  local_addr.sin_port = htons(PORT);
  local_addr.sin_addr.s_addr = INADDR_ANY;
  bzero(&(local_addr.sin_zero),8);
  //bind socket
  ret = bind(socket_fd,(struct  sockaddr*)&local_addr,sizeof(struct sockaddr_in));
  if(ret == -1 ) 
  {
    perror("bind error");
    exit(1);
  }

  ret = listen(socket_fd,backlog);
  if ( ret == -1 )
  {
    perror("listen error");
    exit(1);
  }
	// fill(maccept_fdVec.begin(), maccept_fdVec.end(), -1);
}

CTcpSendThread::~CTcpSendThread() 
{
	// for(const int& accept_fd : maccept_fdVec)
  // {
  //   close(maccept_fdVec[i]);
  // }
	for (int i; i < mDstNum; i++)
	{
		close(maccept_fdVec[i]);
	}
  close(socket_fd);
}

void CTcpSendThread::DivideGroup(const vector<Point2f> &pts)
{
  vector<vector<Point2f>> pts_group;
  pts_group.resize(mDstNum);
  for(const Point2f& pt:pts)
  {
    if (pt.x < 0 || pt.x > 1920 || pt.y < 0 || pt.y > 1080) continue;
    if(pt.x>0 && pt.x<int(1920/5)) pts_group[0].push_back(pt);
    else if (pt.x < int(1920*2/5)) pts_group[1].push_back(pt);
    else if (pt.x < int(1920*3/5)) pts_group[2].push_back(pt);
    else if (pt.x < int(1920*4/5)) pts_group[3].push_back(pt);
    else if (pt.x < 1920) pts_group[4].push_back(pt);
  }  
  for(int i=0; i< mDstNum; ++i)
  {
    if(pts_group[i].size()>0) 
    {
      TcpMsgSender(pts_group[i], i);
    }
  }
}

void CTcpSendThread::TcpMsgSender(const vector<Point2f> &pts, int idx)
{
  int len = pts.size();
  int step = 3, start = 0, end = 0;
  while (len > start) 
  {
    end = min(start + step, len);
    vector<Point2f> tmp(pts.begin() + start, pts.begin() + end);
    SendPoints(tmp, idx);
    start += step;
  }
}

void CTcpSendThread::SendPoints(const vector<Point2f> &pts, int idx)
{
  memset(mSendBuf, 0, sizeof(mSendBuf));
  mSendBuf[0] = 0x55;
  mSendBuf[1] = 0xaa;
  char low, high;
  for (int i = 0; i < 3 && i < pts.size(); ++i) 
  {
    low = (char)((int)(pts[i].x) & 0xff);
    high = (char)(((int)(pts[i].x) & 0xff00) >> 8);
    mSendBuf[i * 4 + 2] = high;
    mSendBuf[i * 4 + 3] = low;
    low = (char)((int)(pts[i].y) & 0xff);
    high = (char)(((int)(pts[i].y) & 0xff00) >> 8);
    mSendBuf[i * 4 + 4] = high;
    mSendBuf[i * 4 + 5] = low;
  }
  mSendBuf[14] = 0x00;
  mSendBuf[15] = 0x00;
  mSendBuf[16] = 0x00;
  mSendBuf[17] = 0xd0;
  mSendBuf[18] = 0xf0;

  // Use TCP to send points
  int accept_fd;
	accept_fd = maccept_fdVec[idx];
	if (accept_fd != -1)
	{
    if (CheckTcpAlive(accept_fd))
    {
      send(accept_fd, (void*)mSendBuf, MESSAGE_SIZE, 0);
    }
	}
}

int CTcpSendThread::CheckTcpAlive(int accept_fd) 
{ 
  if(accept_fd <= 0) 
    return 0; 
  struct tcp_info info; 
  int len=sizeof(info); 
  getsockopt(accept_fd, IPPROTO_TCP, TCP_INFO, &info, (socklen_t *)&len); 
  if((info.tcpi_state == TCP_ESTABLISHED)) 
  { 
    //myprintf("socket connected\n"); 
    return 1;
  } 
  else 
  { 
    //myprintf("socket disconnected\n"); 
    return 0; 
  } 
}

void CTcpSendThread::Stop() 
{
  mShouldExit = true;
  SimpleThread::Stop();
}

int CTcpSendThread::Run() 
{
	int accept_fd = -1;
  int addr_len = sizeof( struct sockaddr_in );
  while (1)
	{
		accept_fd = accept( socket_fd, (struct sockaddr *)&remote_addr, (socklen_t *)&addr_len );
		char * remote_ip = inet_ntoa(remote_addr.sin_addr);
		for(int i=0; i< mDstNum; ++i)
		{
			if (strcmp(remote_ip, addr[i]) == 0)
			{
				maccept_fdVec[i] = accept_fd;
			}
		}
	}
  return 0;
}

#endif